/********** Local Includes ****************************************************/

#include "simpledrivecontroller.h"

/********** Namespaces ********************************************************/

using namespace cv;
using namespace std;

/********** Macros and Constants **********************************************/

#define DEFAULT_SPEED 4
#define DRIVE_THRESHOLD 30
#define TURN_THRESHOLD 30
#define STOP_THRESHOLD 30

/********** Public Methods ****************************************************/

SimpleDriveController::SimpleDriveController() : m_speed(DEFAULT_SPEED) {}

SimpleDriveController::SimpleDriveController(int speed) : m_speed(speed) {}

Command SimpleDriveController::nextOmni(RobotOrientation robot, cv::Point goal) {

    double goalAngle;
    double dx = goal.x - robot.point.x;
    double dy = robot.point.y - goal.y;
    double distance = euclidean(robot.point, goal);
    int oct = 0;
    int time = 300;
    Command command;

    //dx = -dx;


    double robotAngle = 360-robot.angle;

    goalAngle = atan2(dy, dx);
    cout << "whole Angle: " << goalAngle*180/M_PI << " robotAngle: " << robotAngle << endl;
    goalAngle = goalAngle - (robotAngle)*M_PI/180;

    cout << "goalAngle: " << goalAngle << endl;

    //have values now from -pi to +pi
    //want [-pi/8, pi/8] to become [0, pi/4] so the math works below
    goalAngle = goalAngle + M_PI/8;

    while( goalAngle <0 || goalAngle >= 2*M_PI)
    {
        if(goalAngle<0) {
            goalAngle += 2*M_PI;
        }
        if(goalAngle >= 2*M_PI) {
            goalAngle -= 2*M_PI;
        }

    }


    //goalAngle -= M_PI;
    //if(goalAngle<0)
    //    goalAngle += 2*M_PI;


    //divide the unit circle into 8 regions
    oct = floor(goalAngle / (M_PI/4));
    cout << oct << endl;
    switch(oct){
        case -1:
        command.direction = DIR_FORWARD;
        break;
        case 0:
        command.direction = DIR_FORWARD;
        break;
        case 1:
        command.direction = DIR_DIAG_FOR_LEFT;
        break;
        case 2:
        command.direction = DIR_LEFT;
        break;
        case 3:
        command.direction = DIR_DIAG_BACK_LEFT;
        break;
        case 4:
        command.direction = DIR_BACKWARD;
        break;
        case 5:
        command.direction = DIR_DIAG_BACK_RIGHT;
        break;
        case 6:
        command.direction = DIR_RIGHT;
        break;
        case 7:
        command.direction = DIR_DIAG_FOR_RIGHT;
        break;
        default:
        command.direction = HEAD_UP;
    }
    //if near enough, stop.
    if (distance < STOP_THRESHOLD) {
		command.direction = DIR_STOP;
		time = 0;
    }

    return Command(command.direction, speed(), time);
}

Command SimpleDriveController::next(RobotOrientation robot,
	Point goal)
{
	double goalAngle;

	if (robot.point.x == goal.x) {
		if (robot.point.y > goal.y) {
			goalAngle = 0;
		} else {
			goalAngle = 180;
		}
	} else {
		double dx = goal.x - robot.point.x;
		double dy = goal.y - robot.point.y;

		goalAngle = atan2(dy, dx) / M_PI * 180;

		if (goalAngle < 0) {
			goalAngle += 360;
		}
	}

	DriveDirection command;
	int time;
	double distance = euclidean(robot.point, goal);

	if (distance < DRIVE_THRESHOLD) {
		command = DIR_STOP;
		time = 0;
	} else if (fabs(robot.angle - goalAngle) < TURN_THRESHOLD) {
		command = DIR_FORWARD;

		if (distance > 200) {
			time = 2000;
		} else if (distance > 100) {
			time = 1500;
		} else {
			time = 1000;
		}
	//} else if (robot.angle < goalAngle) {
	//	command = DIR_ROT_RIGHT;
	//	time = 200;
	} else {
		command = DIR_ROT_LEFT;
		time = 100;
	}

	return Command(command, 1, time);
}

void SimpleDriveController::setSpeed(int speed) {
	m_speed = speed;
}

int SimpleDriveController::speed() const {
	return m_speed;
}

/********** Private Methods ***************************************************/

double SimpleDriveController::euclidean(Point one, Point two) const {
	double dx = two.x - one.x;
	double dy = two.y - one.y;

	return sqrt(dx * dx + dy * dy);
}
